from ros2webserver.baseserver.protocol import Protocol
from ros2webserver.capabilities.nav_get_odom import NavGetOdom
from ros2webserver.capabilities.nav_cmd_vel import NavCmdVel
from ros2webserver.capabilities.nav_startup import NavStartUp
from ros2webserver.capabilities.nav_initpose import NavInitPose
from ros2webserver.capabilities.nav_to_pose import Nav2Pose
from ros2webserver.capabilities.camera_to_stream import CameraToStream
from ros2webserver.capabilities.sensor_laserscan import LaserScanSubscriber


class Ros2Protocol(Protocol):
    """Adds the handlers for the rosbridge opcodes"""

    rosbridge_capabilities = [
        NavGetOdom,
        NavCmdVel,
        NavStartUp,
        NavInitPose,
        Nav2Pose,
        CameraToStream,
        LaserScanSubscriber
    ]

    print("registered capabilities (classes):")
    for cap in rosbridge_capabilities:
        print(" -", str(cap))

    parameters = None

    def __init__(self, client_id, node_handle,nav_handle=None, parameters=None):
        self.parameters = parameters
        Protocol.__init__(self, client_id, node_handle,nav_handle=nav_handle)
        for capability_class in self.rosbridge_capabilities:
            self.add_capability(capability_class)
